#include <WiFi.h>
#include <WiFiClient.h>
#include <HardwareSerial.h>
#include "../core/AiCamera/servocameracontrol.h"
#include "../core/OdriveControl/odrivecontrol.h"

/*******************************
 *        宏定义
 *******************************/
#define DEBUG_SERIAL_BAUD 115200  // 调试串口波特率

/*******************************
 *        结构体
 *******************************/
// 可在此处定义需要的结构体

/*******************************
 *        常量定义
 *******************************/
// Wi-Fi热点配置
const char* SSID = "高尔夫球拾取车";
const char* PASSWORD = "12345678";

// TCP服务器配置
const int TCP_PORT = 8888;

/*******************************
 *        全局变量
 *******************************/
WiFiServer tcpServer(TCP_PORT);  // TCP服务器实例

/*******************************
 *        初始化函数
 *******************************/
void setup() {
  // 初始化调试串口
  Serial.begin(DEBUG_SERIAL_BAUD);
  INIT_AiCamera
  INIT_ODRIVE_SERIAL
  // 初始化Wi-Fi热点
  initializeWiFiAP();
  
  // 初始化TCP服务器
  initializeTCPServer();
  
  // 初始化ODrive控制器
  initializeODrive();
}

/*******************************
 *        Wi-Fi相关函数
 *******************************/
/**
 * @brief 初始化Wi-Fi热点
 */
void initializeWiFiAP() {
  if (WiFi.softAP(SSID, PASSWORD)) {
    Serial.println("Wi-Fi热点已启动");
    
    // 获取并打印IP地址
    IPAddress ip = WiFi.softAPIP();
    Serial.print("AP IP地址: ");
    Serial.println(ip);
  } else {
    Serial.println("Wi-Fi热点启动失败!");
  }
}

/*******************************
 *        TCP服务器相关函数
 *******************************/
/**
 * @brief 初始化TCP服务器
 */
void initializeTCPServer() {
  tcpServer.begin();
  Serial.println("TCP服务器已启动");
  Serial.print("监听端口: ");
  Serial.println(TCP_PORT);
}

/**
 * @brief 处理客户端连接
 */
void handleClientConnection() {
  // 使用accept()替代已弃用的available()
  WiFiClient client = tcpServer.accept();
  
  if (client) {
    Serial.println("新客户端连接");
    printClientInfo(client);
    
    while (client.connected()) {
      if (client.available()) {
        String command = client.readStringUntil('\n');
        command.trim();
        
        if (!command.isEmpty()) {
          processCommand(command);
        }
      }
    }
    
    client.stop();
    Serial.println("客户端已断开连接");
  }
}

/**
 * @brief 打印客户端信息
 * @param client 客户端对象
 */
void printClientInfo(WiFiClient& client) {
  Serial.print("客户端IP: ");
  Serial.println(client.remoteIP());
  Serial.print("客户端端口: ");
  Serial.println(client.remotePort());
}

/*******************************
 *        ODrive相关函数
 *******************************/
// 注: ODrive相关函数已移至odrivecontrol.h头文件中

/*******************************
 *        命令处理函数
 *******************************/
/**
 * @brief 处理接收到的命令
 * @param command 命令字符串
 */
void processCommand(String command) {
  Serial.print("收到命令: ");
  Serial.println(command);

  if (command.startsWith("set_combined_speed ")) {
    handleCombinedSpeedCommand(command);
  } 
  else if (command.startsWith("set_speed ")) {
    handleSingleAxisSpeedCommand(command);
  } 
  else if (command.startsWith("custom_command ")) {
    handleCustomCommand(command);
  }
  else {
    Serial.println("未知命令");
  }
}

/**
 * @brief 处理组合速度命令
 * @param command 命令字符串
 */
void handleCombinedSpeedCommand(String command) {
  Serial.println("this");
  int spacePos1 = command.indexOf(' ');
  int spacePos2 = command.indexOf(' ', spacePos1 + 1);
  
  if (spacePos1 != -1 && spacePos2 != -1) {
    float speed0 = command.substring(spacePos1 + 1, spacePos2).toFloat();
    float speed1 = command.substring(spacePos2 + 1).toFloat();
    
    setODriveVelocity(speed0, speed1);
  } else {
    Serial.println("无效的组合速度命令格式");
    Serial.println("正确格式: set_combined_speed <speed0> <speed1>");
  }
}

/**
 * @brief 处理单轴速度命令
 * @param command 命令字符串
 */
void handleSingleAxisSpeedCommand(String command) {
  int spacePos1 = command.indexOf(' ');
  int spacePos2 = command.indexOf(' ', spacePos1 + 1);
  
  if (spacePos1 != -1 && spacePos2 != -1) {
    int axis = command.substring(spacePos1 + 1, spacePos2).toInt();
    float speed = command.substring(spacePos2 + 1).toFloat();
    
    String cmd = "w axis" + String(axis) + ".controller.input_vel " + String(speed);
    sendODriveCommand(cmd);
  } else {
    Serial.println("无效的单轴速度命令格式");
    Serial.println("正确格式: set_speed <axis> <speed>");
  }
}

/**
 * @brief 处理自定义命令
 * @param command 命令字符串
 */
void handleCustomCommand(String command) {
  String customCmd = command.substring(15);
  if (customCmd.startsWith("model_h_v"))
  {
    search_object(model_h_v);
    return;
  }
  else if (customCmd.startsWith("reset"))
  {
    servo_reset();
    return;
  }
  else if (customCmd.startsWith("model_v_h"))
  {
    search_object(model_v_h);
    return;
  }  
  else if (customCmd.startsWith("go_")) {
  // 提取
  String params = customCmd.substring(3); 
  // 分割字符串，找到 "_" 的位置
  int underscore_pos = params.indexOf('_');
  if (underscore_pos != -1) {
    // 提取时间部分
    String time_str = params.substring(0, underscore_pos);
    int go_time = time_str.toInt();
    // 提取速度部分
    String speed_str = params.substring(underscore_pos + 1);
    float speed = speed_str.toFloat();
    // 调用 MoveDistance
    MoveDistance(D_FRONT, go_time, speed);
  } else {
    Serial.println("错误：命令格式应为 go_<time>_<speed>");
  }
  return;
}
  else if (customCmd.startsWith("Ag_")) {
  // 提取
  String params = customCmd.substring(3); 
  // 分割字符串，找到 "_" 的位置
  int underscore_pos = params.indexOf('_');
  if (underscore_pos != -1) {
    // 提取时间部分
    String time_str = params.substring(0, underscore_pos);
    int time_to_angle = time_str.toInt();
    // 提取方向部分
    String direction_str = params.substring(underscore_pos + 1);
    float direction = direction_str.toInt();
    // 调用 Angle_of_rotation
    Angle_of_rotation(direction,time_to_angle);
  } else {
    Serial.println("错误：命令格式应为 Ag_<time>_<speed>");
  }
  return;
}
  sendODriveCommand(customCmd);
}

/*******************************
 *        主循环
 *******************************/
void loop() {
  handleClientConnection();
    /*===== 处理串口命令 =====*/
  while(Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n'); // 读取完整指令
    if (cmd.startsWith("model_h_v"))
    {
      search_object(model_h_v);
    }
    else if (cmd.startsWith("reset"))
    {
      servo_reset();
    }
    else if (cmd.startsWith("model_v_h"))
    {
      search_object(model_v_h);
    }
    else break;
  }
}